#include "offboard.hpp"

//位置控制pid
float P_pos = 0.0;
float I_pos = 0.0;
float D_pos = 0.0;
const float I_VEL_LIMIT = 4.0; 
const float D_VEL_LIMIT = 2.0;
const float YAW_RATE_LIMIT = 30.0;

const float YAW_I_LIMIT = 2.0;

const float P_yaw = 3.00;
const float I_yaw = 0.00;
const float D_yaw = 0.00;
const float P_z = 1.00;

int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;
int channel9_value;
int channel10_value;
int channel7_temp = 0;

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

int vehicle_id;

#define POS_I_DEAD_ZONE 4.0 //in dead zone, don't intergrate
#define YAW_DEAD_ZONE 2
#define HEIGHT_DEAD_ZONE 0.0001

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
    juedui_z = msg.monotonic;
}

void odometry_callback(const nav_msgs::Odometry &current_info)
{
    current_point.x= current_info.pose.pose.position.x;
    current_point.y= current_info.pose.pose.position.y;
    current_point.z= current_point_z;
    current_angle.x= current_info.pose.pose.orientation.x;
    current_angle.y= current_info.pose.pose.orientation.y;
    current_angle.z= current_info.pose.pose.orientation.z;
    current_angle.w= current_info.pose.pose.orientation.w;
    curr_angle = toEulerAngle(current_angle);

    current_yaw_angle = curr_angle.z;
    local_odom_flag = true;
}

void velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vel.x = msg.twist.linear.x;
    current_vel.y = msg.twist.linear.y;
    current_vel.z = msg.twist.linear.z;
    xyz_vel = sqrt(current_vel.x*current_vel.x+current_vel.y*current_vel.y+current_vel.z*current_vel.z);
}

void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel5_value = rcvalue.channels[4];
    channel6_value = rcvalue.channels[5];
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
    channel9_value = rcvalue.channels[8];
    channel10_value = rcvalue.channels[9];
}

void gps_callback(const sensor_msgs::NavSatFix &msg)
{
    self_gps_pos.latitude = msg.latitude;
    self_gps_pos.longitude = msg.longitude;
    //use monotonic height
    self_gps_pos.altitude = current_point_z;
}

void master_callback(const geometry_msgs::PoseStamped &msg)
{
    time_master_update = ros::Time::now().toSec();
    //subscribe from master and follow it one by one(maybe)
    master_gps_pos.latitude  = msg.pose.position.y;
    master_gps_pos.longitude = msg.pose.position.x;
    master_gps_pos.altitude  = msg.pose.position.z;
    master_angle_yaw = msg.pose.orientation.w;
    master_vx = msg.pose.orientation.x;
    master_vy = msg.pose.orientation.y;
    formation_mode = std::stoi(msg.header.frame_id);
    master_gps_update_flag = true;
}

void drone2_callback(const geometry_msgs::PoseStamped &msg)
{
    drone2_gps_pos.latitude  = msg.pose.position.y;
    drone2_gps_pos.longitude = msg.pose.position.x;
    drone2_gps_pos.altitude  = msg.pose.position.z;
    drone2_update_flag = true;
}

void drone3_callback(const geometry_msgs::PoseStamped &msg)
{
    drone3_gps_pos.latitude  = msg.pose.position.y;
    drone3_gps_pos.longitude = msg.pose.position.x;
    drone3_gps_pos.altitude  = msg.pose.position.z;
    drone3_update_flag = true;
}

void drone4_callback(const geometry_msgs::PoseStamped &msg)
{
    drone4_gps_pos.latitude  = msg.pose.position.y;
    drone4_gps_pos.longitude = msg.pose.position.x;
    drone4_gps_pos.altitude  = msg.pose.position.z;
    drone4_update_flag = true;
}

void drone5_callback(const geometry_msgs::PoseStamped &msg)
{
    drone5_gps_pos.latitude  = msg.pose.position.y;
    drone5_gps_pos.longitude = msg.pose.position.x;
    drone5_gps_pos.altitude  = msg.pose.position.z;
    drone5_update_flag = true;
}


void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    if(print_state_cnt>100)
    {
        //print state per second
        printFSMExecState();
        print_state_cnt = 0;
    }
    print_state_cnt++;

    //update vector between drones
    if(master_gps_update_flag)
    {
        if(drone2_update_flag)
        {
            vector_1_2 = drone_force_vector(master_gps_pos, drone2_gps_pos);
            dist_1_2 = vector_1_2.norm();
        }
        if(drone3_update_flag)
        {
            vector_1_3 = drone_force_vector(master_gps_pos, drone3_gps_pos);
            dist_1_3 = vector_1_3.norm();
        }
        if(drone4_update_flag)
        {
            vector_1_4 = drone_force_vector(master_gps_pos, drone4_gps_pos);
            dist_1_4 = vector_1_4.norm();
        }
        if(drone5_update_flag)
        {
            vector_1_5 = drone_force_vector(master_gps_pos, drone5_gps_pos);
            dist_1_5 = vector_1_5.norm();
        }
    }
    if(drone2_update_flag)
    {
        if(master_gps_update_flag)
        {
            vector_2_1 = drone_force_vector(drone2_gps_pos, master_gps_pos);
        }
        if(drone3_update_flag)
        {
            vector_2_3 = drone_force_vector(drone2_gps_pos, drone3_gps_pos);
            dist_2_3 = vector_2_3.norm();
        }
        if(drone4_update_flag)
        {
            vector_2_4 = drone_force_vector(drone2_gps_pos, drone4_gps_pos);
            dist_2_4 = vector_2_4.norm();
        }
        if(drone5_update_flag)
        {
            vector_2_5 = drone_force_vector(drone2_gps_pos, drone5_gps_pos);
            dist_2_5 = vector_2_5.norm();
        }
    }
    if(drone3_update_flag)
    {
        if(master_gps_update_flag)
        {
            vector_3_1 = drone_force_vector(drone3_gps_pos, master_gps_pos);
        }
        if(drone2_update_flag)
        {
            vector_3_2 = drone_force_vector(drone3_gps_pos, drone2_gps_pos);
        }
        if(drone4_update_flag)
        {
            vector_3_4 = drone_force_vector(drone3_gps_pos, drone4_gps_pos);
            dist_3_4 = vector_3_4.norm();
        }
        if(drone5_update_flag)
        {
            vector_3_5 = drone_force_vector(drone3_gps_pos, drone5_gps_pos);
            dist_3_5 = vector_3_5.norm();
        }
    }
    if(drone4_update_flag)
    {
        if(master_gps_update_flag)
        {
            vector_4_1 = drone_force_vector(drone4_gps_pos, master_gps_pos);
        }
        if(drone2_update_flag)
        {
            vector_4_2 = drone_force_vector(drone4_gps_pos, drone2_gps_pos);
        }
        if(drone3_update_flag)
        {
            vector_4_3 = drone_force_vector(drone4_gps_pos, drone3_gps_pos);
        }
        if(drone5_update_flag)
        {
            vector_4_5 = drone_force_vector(drone4_gps_pos, drone5_gps_pos);
            dist_4_5 = vector_4_5.norm();
        }
    }
    if(drone5_update_flag)
    {
        if(master_gps_update_flag)
        {
            vector_5_1 = drone_force_vector(drone5_gps_pos, master_gps_pos);
        }
        if(drone2_update_flag)
        {
            vector_5_2 = drone_force_vector(drone5_gps_pos, drone2_gps_pos);
        }
        if(drone3_update_flag)
        {
            vector_5_3 = drone_force_vector(drone5_gps_pos, drone3_gps_pos);
        }
        if(drone4_update_flag)
        {
            vector_5_4 = drone_force_vector(drone5_gps_pos, drone4_gps_pos);
        }
    }

    switch (exec_state) { 
    case INIT: {
        if (!local_odom_flag) {
            break;
        }
        changeFSMExecState(CHECK_ID, "offboard_fsm");
    break;
    }

    case CHECK_ID: {
        if(vehicle_id==1)
        {
            changeFSMExecState(LEADER, "offboard_fsm");
        }
        else if(vehicle_id>1)
        {
            changeFSMExecState(FOLLOWER, "offboard_fsm");
        }
    break;
    }

    case LEADER: {
        // std::cout<<"---->this is the leader vehicle.."<<std::endl;
        
    break;
    }
    case FOLLOWER: {
        sensor_msgs::NavSatFix offset_gps_pos;  
        // the pose -1.57 is on left and +1.57 is on right
        double target_angle_yaw = 0;

        if(formation_mode==1)  //straight line formate
        {
            float offset = (vehicle_id-1)*6e-5;
            offset_gps_pos.latitude = master_gps_pos.latitude-offset*sin(master_angle_yaw+1.57);
            offset_gps_pos.longitude = master_gps_pos.longitude-offset*cos(master_angle_yaw+1.57);
            offset_gps_pos.altitude = master_gps_pos.altitude;
            target_angle_yaw = master_angle_yaw;
            // std::cout<<"======>target_gps: "<<offset_gps_pos.latitude<<","<<offset_gps_pos.longitude<<","<<offset_gps_pos.altitude<<std::endl;
            // std::cout<<"======>self_gps: "<<self_gps_pos.latitude<<","<<self_gps_pos.longitude<<","<<self_gps_pos.altitude<<std::endl;
            gps_position_pid_control(offset_gps_pos,self_gps_pos,xyz_vel+1,target_angle_yaw);
            vel_command_send(velocity_expected, attitude_expect.z);
            
        }
        else if(formation_mode==2)
        {
            float offset = (vehicle_id-1)*2e-4;
            offset_gps_pos.latitude = master_gps_pos.latitude-offset*sin(master_angle_yaw+1.57);
            offset_gps_pos.longitude = master_gps_pos.longitude-offset*cos(master_angle_yaw+1.57);
            offset_gps_pos.altitude = master_gps_pos.altitude;
            target_angle_yaw = master_angle_yaw;
            // std::cout<<"======>target_gps: "<<offset_gps_pos.latitude<<","<<offset_gps_pos.longitude<<","<<offset_gps_pos.altitude<<std::endl;
            // std::cout<<"======>self_gps: "<<self_gps_pos.latitude<<","<<self_gps_pos.longitude<<","<<self_gps_pos.altitude<<std::endl;
            gps_position_pid_control(offset_gps_pos,self_gps_pos,xyz_vel+1,target_angle_yaw);
            vel_command_send(velocity_expected, attitude_expect.z);

        }
        else if(formation_mode==3)
        {
            float offset_lefright;
            float offset_forwardback;
            float offset_dist;
            float angle_formation;
            
            switch (vehicle_id)
            {
            case 2:{
                offset_lefright = (vehicle_id-1)*6;
                offset_forwardback = 10;
                offset_dist = sqrt(offset_lefright*offset_lefright+offset_forwardback*offset_forwardback);
                angle_formation = abs(atan2(offset_lefright, offset_forwardback));
                offset_gps_pos.latitude = master_gps_pos.latitude-offset_dist*sin(master_angle_yaw+angle_formation);
                offset_gps_pos.longitude = master_gps_pos.longitude-offset_dist*cos(master_angle_yaw+angle_formation);
                break;
            }
            case 3:{
                offset_lefright = (vehicle_id-1)*6;
                offset_forwardback = 20;
                offset_dist = sqrt(offset_lefright*offset_lefright+offset_forwardback*offset_forwardback);
                angle_formation = abs(atan2(offset_lefright, offset_forwardback));
                offset_gps_pos.latitude = master_gps_pos.latitude-offset_dist*sin(master_angle_yaw+angle_formation);
                offset_gps_pos.longitude = master_gps_pos.longitude-offset_dist*cos(master_angle_yaw+angle_formation);
                break;
            }
            case 4:{
                offset_lefright = (vehicle_id-1)*6;
                offset_forwardback = 34;
                offset_dist = sqrt(offset_lefright*offset_lefright+offset_forwardback*offset_forwardback);
                angle_formation = abs(atan2(offset_lefright, offset_forwardback));
                offset_gps_pos.latitude = master_gps_pos.latitude-offset_dist*sin(master_angle_yaw+angle_formation);
                offset_gps_pos.longitude = master_gps_pos.longitude-offset_dist*cos(master_angle_yaw+angle_formation);
                break;
            }
            case 5:{
                offset_lefright = (vehicle_id-1)*6;
                offset_forwardback = 72;
                offset_dist = sqrt(offset_lefright*offset_lefright+offset_forwardback*offset_forwardback);
                angle_formation = abs(atan2(offset_lefright, offset_forwardback));
                offset_gps_pos.latitude = master_gps_pos.latitude-offset_dist*sin(master_angle_yaw+angle_formation);
                offset_gps_pos.longitude = master_gps_pos.longitude-offset_dist*cos(master_angle_yaw+angle_formation);
                break;
            }

            default:
                break;
            }


            offset_gps_pos.altitude = master_gps_pos.altitude;
            target_angle_yaw = master_angle_yaw;
            gps_position_pid_control(offset_gps_pos,self_gps_pos,xyz_vel+1,target_angle_yaw);
            vel_command_send(velocity_expected, attitude_expect.z);

        }
        
    break;
    }

    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test");
    ros::NodeHandle nh;
    double P_pos_read, I_pos_read, D_pos_read;
    //read parameters from launch file
    nh.param<double>("P_pos",P_pos_read,1.0);
    nh.param<double>("I_pos",I_pos_read,0.2);
    nh.param<double>("D_pos",D_pos_read,0.1);
    nh.param<int>("vehicle_id",vehicle_id,-1);
    nh.param<double>("master_angle_yaw",master_angle_yaw,1.57);
    
    P_pos = P_pos_read;
    I_pos = I_pos_read;
    D_pos = D_pos_read;

    //subscribe self topics
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    odometrysub     = nh.subscribe("/mavros/local_position/odom",10,odometry_callback);
    velocitysub     = nh.subscribe("/mavros/local_position/velocity_local",10,velocity_callback);
    gpssub     = nh.subscribe("/mavros/global_position/global",10,gps_callback);
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);
    state_sub = nh.subscribe("/mavros/state", 10, state_cb);

    //subscribe other vehicle pose
    master_sub = nh.subscribe("/drone1/local_position/pose", 10, master_callback);
    drone2_sub = nh.subscribe("/drone2/local_position/pose", 10, drone2_callback);
    drone3_sub = nh.subscribe("/drone3/local_position/pose", 10, drone3_callback);
    drone4_sub = nh.subscribe("/drone4/local_position/pose", 10, drone4_callback);
    drone5_sub = nh.subscribe("/drone5/local_position/pose", 10, drone5_callback);

    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);

    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    land_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("/mavros/setpoint_velocity/mav_frame");

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.01), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"waiting for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    mavros_msgs::SetMavFrame setmavframe;
    setmavframe.request.mav_frame = 8;

    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}


void gps_position_pid_control(sensor_msgs::NavSatFix set_gps_pos, sensor_msgs::NavSatFix current_gps_pos, float velocity_limit, float target_yaw)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim = 0.0, vy_lim = 0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0, pitch = 0.0;
    //publish plot param
    std_msgs::Float32MultiArray msg1;
    //position control  mode & 0x01
    if (1) {
        //calculate velocity, P control
        geometry_msgs::Point error_pos;
        error_pos.x = (set_gps_pos.longitude - current_gps_pos.longitude)*111318.0*cos((set_gps_pos.latitude + current_gps_pos.latitude)/2*deg2rad);
        error_pos.y = (set_gps_pos.latitude - current_gps_pos.latitude)*110946.0; // gain needs to be same scale jiaxing suggest to use accurate equation 
        msg1.data.push_back(error_pos.x);   
        msg1.data.push_back(error_pos.y); 
        //to be sure height safety 
        if(current_point.z>2.0)
        {
            error_pos.z = set_gps_pos.altitude - current_gps_pos.altitude;
        }
        else
        {
            error_pos.z = 1.5 - current_point.z;
        }
        vz = P_z * error_pos.z;
        msg1.data.push_back(error_pos.z);
        
        vxp = P_pos * error_pos.x;
        vyp = P_pos * error_pos.y;
        // vxd = D_pos * (error_pos.x - error_pos_last.x)*10.0;
        // vyd = D_pos * (error_pos.y - error_pos_last.y)*10.0;

        vxd = -(D_pos * current_vel.x);
        vyd = -(D_pos * current_vel.y);

        vxd = limit(vxd, D_VEL_LIMIT);
        vyd = limit(vyd, D_VEL_LIMIT);

        if (abs(error_pos.x) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.x += error_pos.x*0.1;
        } else {
            error_pos_integrated.x = 0.0;
        }
        if (abs(error_pos.y) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.y += error_pos.y*0.1;
        } else {
            error_pos_integrated.y = 0.0;
        }
        if (I_pos > 0.0001) {
            error_pos_integrated.x = limit((float) error_pos_integrated.x, I_VEL_LIMIT / I_pos);
            error_pos_integrated.y = limit((float) error_pos_integrated.y, I_VEL_LIMIT / I_pos);
        }
        vxi = I_pos * error_pos_integrated.x;
        vyi = I_pos * error_pos_integrated.y;

        //publish vxi vxd
        msg1.data.push_back(master_vx);   
        msg1.data.push_back(master_vy);    
        msg1.data.push_back(current_vel.x);   
        msg1.data.push_back(current_vel.y); 

        vx = (vxp + vxi + vxd);
        vy = (vyp + vyi + vyd);


        //limit the speed
        vx_lim = limit_velocity(vx, vy, velocity_limit).x;
        vy_lim = limit_velocity(vx, vy, velocity_limit).y;
        //ROS_INFO("vx_exp vy_exp %f %f",vx,vy);

        

        error_pos_last = error_pos;
    }
    //yaw control  mode & 0x02
    if (1) {
        //use the flight controller yaw in default
        
        float current_yaw = current_yaw_angle;
        //Drone turns at the smallest angle
        float error_yaw = (target_yaw - current_yaw) * rad2deg;
        if (error_yaw < -180)
            error_yaw += 360;
        else if (error_yaw > 180)
            error_yaw -= 360;
        yaw_rate_p = P_yaw * error_yaw;
        yaw_rate_d = D_yaw * (error_yaw - error_yaw_last);
        error_yaw_integrated += error_yaw;
        if (I_yaw > 0.0001) {
            error_yaw_integrated = limit(error_yaw_integrated, YAW_I_LIMIT / I_yaw);
        }
        yaw_rate_i = I_yaw * error_yaw_integrated;
        if (abs(error_yaw) <= YAW_DEAD_ZONE) {
            yaw_rate_p = 0.0;
            yaw_rate_i = 0.0;
            yaw_rate_d = 0.0;
            error_yaw_integrated = 0.0;
        }
        yaw_rate = (yaw_rate_p + yaw_rate_i + yaw_rate_d);
        if (abs(error_yaw) >= YAW_RATE_LIMIT) {
            yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
        } 
        // else {
        //     yaw_rate = limit(yaw_rate, (float) (YAW_RATE_LIMIT * 0.4));
        // }
        error_yaw_last = error_yaw;
    }

    //机间避让

    int need_birang = 0;
    Eigen::Vector3d final_vector;
    switch (vehicle_id)
    {
    case 2:{
        if(dist_2_3<5)
        {
            final_vector += vector_3_2;
            need_birang++;
        }
        if(dist_2_4<5)
        {
            final_vector += vector_4_2;
            need_birang++;
        }
        if(dist_2_5<5)
        {
            final_vector += vector_5_2;
            need_birang++;
        }

    break;
    }

    case 3:{
        if(dist_2_3<5)
        {
            final_vector += vector_2_3;
            need_birang++;
        }
        if(dist_3_4<5)
        {
            final_vector += vector_4_3;
            need_birang++;
        }
        if(dist_3_5<5)
        {
            final_vector += vector_5_3;
            need_birang++;
        }

    break;
    }

    case 4:{
        if(dist_2_4<5)
        {
            final_vector += vector_2_4;
            need_birang++;
        }
        if(dist_3_4<5)
        {
            final_vector += vector_3_4;
            need_birang++;
        }
        if(dist_4_5<5)
        {
            final_vector += vector_5_4;
            need_birang++;
        }

    break;
    }

    case 5:{
        if(dist_2_5<5)
        {
            final_vector += vector_2_5;
            need_birang++;
        }
        if(dist_3_5<5)
        {
            final_vector += vector_3_5;
            need_birang++;
        }
        if(dist_4_5<5)
        {
            final_vector += vector_4_5;
            need_birang++;
        }

    break;
    }

    }
    if(need_birang>0)
    {
        final_vector = final_vector/need_birang;
        double vector_norm = sqrt(final_vector(0)*final_vector(0)+final_vector(1)*final_vector(1));
        double string_length = 5.0-vector_norm;
        if(string_length<0)
        {
            string_length = 0;
        }
        velocity_expected.x = string_length*final_vector(0)/vector_norm;
        velocity_expected.y = string_length*final_vector(1)/vector_norm;
        velocity_expected.z = vz;
        attitude_expect.z = yaw_rate * deg2rad;
    }
    else
    {
        velocity_expected.x = vx_lim;
        velocity_expected.y = vy_lim;
        velocity_expected.z = vz;
        attitude_expect.z = yaw_rate * deg2rad;
    }
    


}

void vel_command_send(geometry_msgs::Point vel, double yaw_rate)
{
    vel_command_cnt++;
    geometry_msgs::Vector3 linear_temp;
    geometry_msgs::Vector3 angular_temp;

    geometry_msgs::TwistStamped msg_vel_pub;
    linear_temp.x = vel.x;
    linear_temp.y = vel.y;
    linear_temp.z = vel.z;
    
    angular_temp.x = 0;
    angular_temp.y = 0;
    angular_temp.z = yaw_rate;
    msg_vel_pub.header.stamp = ros::Time::now();
    msg_vel_pub.header.seq=vel_command_cnt;
    msg_vel_pub.twist.linear=linear_temp;
    msg_vel_pub.twist.angular=angular_temp;
    local_vel_pub.publish(msg_vel_pub);
}

geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;

    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}


geometry_msgs::Point limit_velocity(float vx, float vy,float maximum)
{
	geometry_msgs::Point vel;
	float velocity = sqrt(vx * vx + vy * vy);
	if(maximum <= 0)
	{
		vel.x = 0;
		vel.y = 0;
	}
	if(velocity <= maximum)
	{
		vel.x = vx;
		vel.y = vy;
	}
	//if velocity is bigger than maximum, then limit the vx and vy, and keep the direction at meanwhile.
	else
	{
		//the velocity must not be zero when in this step
		vel.x = vx / velocity * maximum;
		vel.y = vy / velocity * maximum;
	}
	return vel;
}

Eigen::Vector3d drone_force_vector(sensor_msgs::NavSatFix gps1, sensor_msgs::NavSatFix gps2)
{
    Eigen::Vector3d returnvector;
    returnvector(0) = (gps2.longitude - gps1.longitude)*111318.0*cos((gps2.latitude + gps1.latitude)/2*deg2rad);
    returnvector(1) = (gps2.latitude - gps1.latitude)*110946.0; 
    returnvector(2) = (gps2.altitude-gps1.altitude);

    return returnvector;
}


void changeFSMExecState(FSM_STATE new_state, std::string pos_call) {

  int    pre_s        = int(exec_state);
  exec_state         = new_state;
  std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
  std::cout << "state: " + state_str[int(exec_state)] << std::endl;
}
